﻿#include "SerialPeel.h"
#include <QDebug>
#include <QFile>
#include <QVariantList>
#include <QVariantMap>
#include "JsonUtil.h"

#include "commander.h"

SerialPeel::SerialPeel(int argc, char** argv, QObject* parent)
    : argc_(argc), argv_(argv), QObject(parent)
{
    m_com = new QSerialPort(this);
    connect(m_com, SIGNAL(readyRead()), this, SLOT(onReadReady()));

    m_ros_spin_thread = std::thread{std::bind(&SerialPeel::rosSpinThread, this)};
}

SerialPeel::~SerialPeel()
{
    rclcpp::shutdown();
}

void SerialPeel::write(unsigned char* data, int len)
{
    QByteArray ba((char*)data, len);
    if (nullptr != m_com && m_com->isOpen()) {
        m_com->write(ba);
    }
}

void SerialPeel::setCom(const std::string& comName)
{
    qDebug() << "openCom: " << QString::fromStdString(comName);
    m_com->setPortName(QString::fromStdString(comName));
    m_com->setBaudRate(QSerialPort::Baud9600);
    m_com->setParity(QSerialPort::NoParity);
    m_com->setDataBits(QSerialPort::Data8);
    m_com->setStopBits(QSerialPort::OneStop);
    QMetaObject::invokeMethod(this, "openCom", Qt::QueuedConnection);
}

void SerialPeel::openCom()
{
    bool bOpen = m_com->open(QIODevice::ReadWrite);
    qDebug() << "openCom " << (bOpen ? "OK" : "NG");

    if (bOpen) {
        quint8 data[8]{1, 2, 3, 4, 5, 6, 7, 8};
        write(data, 8);

        m_cmder = new Commander();
    }
}

void SerialPeel::closeCom()
{
    m_com->close();
}

void SerialPeel::call_cmd(const std::string& cmdName)
{
    QMetaObject::invokeMethod(this, cmdName.c_str(), Qt::QueuedConnection);
}

void SerialPeel::reset()
{
    quint8 arrCmd[8] = {0};
    m_cmder->getCmd("reset", arrCmd);

    write(arrCmd, 8);
}

void SerialPeel::pinch()
{
    quint8 arrCmd[8] = {0};
    m_cmder->getCmd("pinch", arrCmd);

    write(arrCmd, 8);
}

void SerialPeel::loose()
{
    quint8 arrCmd[8] = {0};
    m_cmder->getCmd("loose", arrCmd);

    write(arrCmd, 8);
}

void SerialPeel::stop()
{
    quint8 arrCmd[8] = {0};
    m_cmder->getCmd("stop", arrCmd);

    write(arrCmd, 8);
}

void SerialPeel::startPeel(int nCm)
{
    quint8 arrCmd[8] = {0};
    m_cmder->getCmd("startPeel", arrCmd);

    write(arrCmd, 8);
}

void SerialPeel::stopPeel()
{
    quint8 arrCmd[8] = {0};
    m_cmder->getCmd("stopPeel", arrCmd);

    write(arrCmd, 8);
}

void SerialPeel::rosSpinThread()
{
    rclcpp::init(argc_, argv_);
    m_robotCmdNode = std::make_shared<RobotCmdNode>(this);
    rclcpp::spin(m_robotCmdNode);
    rclcpp::shutdown();
}

void SerialPeel::onReadReady()
{
    qDebug() << "onReady redady...............................";
    QByteArray ba = m_com->readAll();
    m_cmder->parseCmd((quint8*)ba.data(), ba.size());
}
